#include <iostream>
#include <cmath>
#include "zhnmat.hpp"
#include "tracelog.h"
#include "quaternions.hpp"
#include "camera.h"
using namespace zhnmat;

Mat curRotation;
Vector3d aeroAngle, eulrAngle;
Model model;
Mat BaseTransform;
////////////////////////////////////////////////////////////////
// 视角控制
////////////////////////////////////////////////////////////////
bool Shift_View() {
    static u8 state = 0x00;
    if (IsKeyDown(KEY_O)) state |= 0x02;
    if (!IsKeyDown(KEY_O) && (state & 0x02)) { state &=~ 0x02; state ^= 0x01; }
    if (state & 0x01) return true;
    else return false;
}

////////////////////////////////////////////////////////////////
// 数据转换
////////////////////////////////////////////////////////////////
Matrix Mat33_to_Matrix(Mat m) {
    return (Matrix){
        (float)m.at(0, 0), (float)m.at(0, 1), (float)m.at(0, 2), 0,
        (float)m.at(1, 0), (float)m.at(1, 1), (float)m.at(1, 2), 0,
        (float)m.at(2, 0), (float)m.at(2, 1), (float)m.at(2, 2), 0,
        0, 0, 0, 1};
}
Vector3 Mat31_to_Vec3(Mat m) {
    return (Vector3){float(m.at(0,0)), float(m.at(1,0)), float(m.at(2,0))};
}
Vector3 Vec3d_to_Vec3(zhnmat::Vector3d vec) {
    return (Vector3){(float)vec._x, (float)vec._y, (float)vec._z};
}

////////////////////////////////////////////////////////////////
// 高阶画图
////////////////////////////////////////////////////////////////
void DrawCirclePlane(Vector3d centerPos, double radius, Vector3d normalVec, Color color) {
    Vector3d vecx = radius * Vector3d(normalVec._y, -normalVec._x, 0).Normalvector();
    Vector3d vecy =  radius * (normalVec & vecx).Normalvector();
    Vector3d point1, point2;
    point1 = vecx + centerPos;
	for(double theta=0.1; theta<=6.28; theta+=0.1) {
        point2 = vecx * cos(theta) + vecy * sin(theta);
        point2 += centerPos;
        DrawTriangle3D(Vec3d_to_Vec3(centerPos), Vec3d_to_Vec3(point1), Vec3d_to_Vec3(point2), color);
        DrawTriangle3D(Vec3d_to_Vec3(centerPos), Vec3d_to_Vec3(point2), Vec3d_to_Vec3(point1), color);
        point1 = point2;
	}
    DrawTriangle3D(Vec3d_to_Vec3(centerPos), Vec3d_to_Vec3(point1), Vec3d_to_Vec3(vecx), color);
    DrawTriangle3D(Vec3d_to_Vec3(centerPos), Vec3d_to_Vec3(vecx), Vec3d_to_Vec3(point1), color);
}
void DrawLine3DBold(Vector3 startPos, Vector3 endPos, Color color) {
    DrawLine3D(Vector3Add(startPos, (Vector3){ 0.01, 0, 0}), Vector3Add(endPos, (Vector3){ 0.01, 0, 0}), color);
    DrawLine3D(Vector3Add(startPos, (Vector3){-0.01, 0, 0}), Vector3Add(endPos, (Vector3){-0.01, 0, 0}), color);
    DrawLine3D(Vector3Add(startPos, (Vector3){0,  0.01, 0}), Vector3Add(endPos, (Vector3){0,  0.01, 0}), color);
    DrawLine3D(Vector3Add(startPos, (Vector3){0, -0.01, 0}), Vector3Add(endPos, (Vector3){0, -0.01, 0}), color);
    DrawLine3D(Vector3Add(startPos, (Vector3){0, 0,  0.01}), Vector3Add(endPos, (Vector3){0, 0,  0.01}), color);
    DrawLine3D(Vector3Add(startPos, (Vector3){0, 0, -0.01}), Vector3Add(endPos, (Vector3){0, 0, -0.01}), color);
}
void Init_RigidBody() {
    model = LoadModel("C:/my/resources/plane.obj");
    Texture2D img = LoadTexture("C:/my/resources/plane.png");
    model.materials[0].maps[MATERIAL_MAP_DIFFUSE].texture = img;
    const double pitchfix = 0.15;
    BaseTransform = Mat(3, 3, vecdble{
        0, -sin(pitchfix), -cos(pitchfix),
        -1, 0, 0,
        0, cos(pitchfix), -sin(pitchfix),
    });
}
void Draw_RigidBody(Mat R) {
    model.transform = Mat33_to_Matrix(R*BaseTransform);
    Mat translation = R*BaseTransform*Mat(vecdble{0, -0.58, 0});
    DrawModel(model, Mat31_to_Vec3(translation), 0.05, WHITE);
}
void Draw_Body_Axes(Mat R) {
    DrawLine3D(Vector3Zero(), Mat31_to_Vec3(R.atc(0) * 3), RED);
    DrawLine3D(Vector3Zero(), Mat31_to_Vec3(R.atc(1) * 3), GREEN);
    DrawLine3D(Vector3Zero(), Mat31_to_Vec3(R.atc(2) * 3), BLUE);
    // DrawLine3D(Vector3Zero(), Mat31_to_Vec3(R.atc(0) * -3), RED);
    // DrawLine3D(Vector3Zero(), Mat31_to_Vec3(R.atc(2) * 3), GREEN);
    // DrawLine3D(Vector3Zero(), Mat31_to_Vec3(R.atc(1) * 3), BLUE);
}
void Draw_World_Axes(Mat R) {
    DrawLine3DBold(Vector3Zero(), (Vector3){10, 0, 0}, RED);
    DrawLine3DBold(Vector3Zero(), (Vector3){0, 10, 0}, GREEN);
    DrawLine3DBold(Vector3Zero(), (Vector3){0, 0, 10}, BLUE);
    // DrawLine3DBold(Vector3Zero(), (Vector3){-10, 0, 0}, RED);
    // DrawLine3DBold(Vector3Zero(), (Vector3){0, 0, 10}, GREEN);
    // DrawLine3DBold(Vector3Zero(), (Vector3){0, 10, 0}, BLUE);
}

////////////////////////////////////////////////////////////////
// 键盘事件
////////////////////////////////////////////////////////////////
void Keyboard_Rotation() {
    // eulrAngle = Rotation_to_Euler(curRotation);
    // if (IsKeyDown(KEY_Q)) eulrAngle._x += 0.01;
    // if (IsKeyDown(KEY_A)) eulrAngle._x -= 0.01;
    // if (IsKeyDown(KEY_W)) eulrAngle._y += 0.01;
    // if (IsKeyDown(KEY_S)) eulrAngle._y -= 0.01;
    // if (IsKeyDown(KEY_E)) eulrAngle._z += 0.01;
    // if (IsKeyDown(KEY_D)) eulrAngle._z -= 0.01;
    // curRotation = Euler_To_Rotation(eulrAngle);
    if (IsKeyDown(KEY_Q)) curRotation *= Rotation_AxisX( 0.01);
    if (IsKeyDown(KEY_A)) curRotation *= Rotation_AxisX(-0.01);
    if (IsKeyDown(KEY_W)) curRotation *= Rotation_AxisY( 0.01);
    if (IsKeyDown(KEY_S)) curRotation *= Rotation_AxisY(-0.01);
    if (IsKeyDown(KEY_E)) curRotation *= Rotation_AxisZ( 0.01);
    if (IsKeyDown(KEY_D)) curRotation *= Rotation_AxisZ(-0.01);
    aeroAngle = Rotation_to_Aeroangle(curRotation);
    if (IsKeyDown(KEY_R)) aeroAngle._x += 0.01;
    if (IsKeyDown(KEY_F)) aeroAngle._x -= 0.01;
    if (IsKeyDown(KEY_T)) aeroAngle._y += 0.01;
    if (IsKeyDown(KEY_G)) aeroAngle._y -= 0.01;
    if (IsKeyDown(KEY_Y)) aeroAngle._z += 0.01;
    if (IsKeyDown(KEY_H)) aeroAngle._z -= 0.01;
    curRotation = Aeroangle_To_Rotation(aeroAngle);
    if (IsKeyDown(KEY_P)) curRotation = eye(3);
}

////////////////////////////////////////////////////////////////
// 主函数
////////////////////////////////////////////////////////////////
int main(void) {
    SetLogLevel(LOG_FATAL);
    Camera camera;
	SetConfigFlags(FLAG_MSAA_4X_HINT);
    SetTargetFPS(60);
    SetWindowMonitor(2);
	SetConfigFlags(FLAG_MSAA_4X_HINT);
	SetConfigFlags(FLAG_FULLSCREEN_MODE);
    SetTargetFPS(50);
    InitGraph(0, 0, "RayLib-3D");
	Init_Camera(&camera);
    Init_RigidBody();

    curRotation = eye(3);  // 当前姿态旋转矩阵
    aeroAngle = eulrAngle = Vector3d(0, 0, 0);  // 气动角和欧拉角

    while (!WindowShouldClose()) {
        Keyboard_Rotation();
        Vector3d vecbx = curRotation.atc(0);  // 本体系x轴
        Vector3d normalTotalAttack = Vector3d(-1, 0, 0) & vecbx;  // 总攻角平面法向量(未归一化)
        Vector3d normalVerticalPlane = Vector3d(0, 0, 1) & vecbx;  // 本体系x轴所在铅垂面法向量(未归一化)
        Vector3d normalVz = Vector3d(curRotation.atc(1)) & Vector3d(-1, 0, 0);  // 速度系z轴(未归一化)
        Vector3d aerofL = (normalTotalAttack & Vector3d(-1, 0, 0)).Normalvector();  // 归一化气动升力向量
        Vector3d aerofN = (normalTotalAttack & vecbx) * 3;  // 总法向力N在总攻角平面内与本体系x轴垂直(未归一化)
        Vector3d centerMass = curRotation * Vector3d(-2, 0, 0.5);  // 质心
        if (normalTotalAttack.norm2() < 1e-3)
            normalTotalAttack = Vector3d(0, 1, 0);
        else
            normalTotalAttack.Normalize();

        Update_Camera(&camera);
        BeginDrawing();
            ClearBackground(RAYWHITE);
            BeginMode3D(camera);
                if (Shift_View())
                    Draw_RigidBody(curRotation);
                DrawGrid(100, 2);
                Draw_Body_Axes(curRotation);
                Draw_World_Axes(curRotation);
                // DrawLine3D(Vector3Zero(), Vec3d_to_Vec3(normalVz), (Color){ 0x00, 0xFF, 0x00, 0xFF });  // 速度系z轴
                DrawLine3D(Vector3Zero(), Vec3d_to_Vec3(aerofL*2), PURPLE);  // 气动升力L
                // DrawLine3D(Vector3Zero(), Vec3d_to_Vec3(aerofN), PURPLE);  // 总法向力N
                // DrawSphereEx(Vec3d_to_Vec3(centerMass), 0.1, 4, 4, BLUE);  // 质心位置
                // DrawCirclePlane(Vector3d(), 3, normalTotalAttack, (Color){ 0x87, 0x3C, 0xBE, 0x20 });  // VIOLET 总攻角平面
                // DrawCirclePlane(Vector3d(), 3, normalVerticalPlane, (Color){ 0x87, 0x3C, 0xBE, 0x20 });  // VIOLET 铅垂面
                DrawCirclePlane(Vector3d(), 3, curRotation.atc(1), (Color){ 0xFF, 0xA1, 0x00, 0x20 });  // ORANGE 纵向对称面
            EndMode3D();
            DrawText(TextFormat("%2i FPS", GetFPS()), 0, 0, 20, LIME);
        EndDrawing();
    }
    CloseGraph();
    return 0;
}
